package zjut.raymond.witdemo.algorithm;

import android.hardware.SensorManager;
import android.util.Log;

import static android.content.ContentValues.TAG;

public class PedestrianDeadReckoningAlgorithm {
    public PedestrianDeadReckoningAlgorithm() {
        initObjects();
    }

    private void initObjects() {
        gyroOrientation[0] = 0.0f;
        gyroOrientation[1] = 0.0f;
        gyroOrientation[2] = 0.0f;

        // Initialize gyroMatrix with identity matrix
        gyroMatrix[0] = 1.0f;
        gyroMatrix[1] = 0.0f;
        gyroMatrix[2] = 0.0f;
        gyroMatrix[3] = 0.0f;
        gyroMatrix[4] = 1.0f;
        gyroMatrix[5] = 0.0f;
        gyroMatrix[6] = 0.0f;
        gyroMatrix[7] = 0.0f;
        gyroMatrix[8] = 1.0f;

        // initialize weightMatrix;
//        weightMatrix[0] = 4.0f;
//        weightMatrix[1] = 2.0f;
//        weightMatrix[2] = 1.0f;
    }


    //回调接口

    public interface Callback {
        public void onLocationChanged(double stepLength, double orientation);

        public void onOrientationChanged(double orientation);
    }

    Callback callback;

    public void setCallback(Callback callback) {
        this.callback = callback;
    }

    private Vector3Event mAccV = new Vector3Event();
    private Vector3Event mMagV = new Vector3Event();
    private Vector3Event mGyrV = new Vector3Event();

    /**
     * 相关参数
     */
    // Window_size
    private static final int ACCELERATION_WINDOW = 5;
    private static final int MAGNETIC_FIELD_WINDOW = 20;
    private static final int STD_WINDOW = 20;
    private static final int ORIENTATION_WINDOW = 10;

    private MovingAverageFilter mAccelerometerFilter_X = new MovingAverageFilter(ACCELERATION_WINDOW);
    private MovingAverageFilter mAccelerometerFilter_Y = new MovingAverageFilter(ACCELERATION_WINDOW);
    private MovingAverageFilter mAccelerometerFilter_Z = new MovingAverageFilter(ACCELERATION_WINDOW);
    private MovingAverageFilter mMagneticFilter_X = new MovingAverageFilter(MAGNETIC_FIELD_WINDOW);
    private MovingAverageFilter mMagneticFilter_Y = new MovingAverageFilter(MAGNETIC_FIELD_WINDOW);
    private MovingAverageFilter mMagneticFilter_Z = new MovingAverageFilter(MAGNETIC_FIELD_WINDOW);
    private MovingAverageFilter mSinFilter = new MovingAverageFilter(ORIENTATION_WINDOW);
    private MovingAverageFilter mCosFilter = new MovingAverageFilter(ORIENTATION_WINDOW);

    private STDCalculator mAccSTDCalculator = new STDCalculator(STD_WINDOW);


    private double accStandardDeviation = 0.0d;

    // Min/Max interval of two steps
    private static final int MIN_STEP_INTERVAL = 450;
    private static final int MIN_RUN_INTERVAL = 350;
    private static final int INT_STEP_INTERVAL = 600;
    private static final int MAX_STEP_INTERVAL = 1800;
    private float dynamicInterval = INT_STEP_INTERVAL;
    private float dynamicMinInterval = MIN_STEP_INTERVAL;

    // Date
    private long mLastStepTime = System.currentTimeMillis();

    private float K = 0.5f;
    private float dynamicStepLength;

    // Orientation
    private boolean hasOrientation = false;
    private boolean initState = false;
    private float[] rotationMatrix = new float[9];
    private float[] outR = new float[9];
    private float mOrientationResult;
    private float[] mOrientationValues = new float[3];// orientation angles from accel and magnet
    private float[] deltaRotationVector = new float[4];// copy the new gyro values into the gyro array convert the raw gyro
    private float[] gyroMatrix = new float[9]; // rotation matrix from gyro data
    private float[] gyroOrientation = new float[3]; // orientation angles from gyro matrix
    private float[] deltaRotationMatrix = new float[9]; // convert rotation vector into rotation matrix
    private float[] fusedOrientation = new float[3]; // final orientation angles from sensor fusion

    //TODO:以下参数需重新测试
    // trigger acceleration of one step (ALWAYS >0)
    private static final float STEP_TRIGGER_ACCEL = 11.0f;
    private float maxSumAcc = 10.5f;
    private float minSumAcc = 9.0f;
    private static final float maxPeakThreshold = 10.5f;
    private static final float minPeakThreshold = 9.0f;


    //互补滤波器系数
    private static final float FILTER_COEFFICIENT = 0.98f;

    public static final float EPSILON = 0.000000001f;
    private static final float NS2S = 1.0f / 1000000000.0f;
    private float timestamp;

    public void inputData(Vector3Event inputAcc, Vector3Event inputMag, Vector3Event inputGyr) {
        //滤波处理
        mAccV.x = mAccelerometerFilter_X.filterProcess(inputAcc.x);
        mAccV.y = mAccelerometerFilter_Y.filterProcess(inputAcc.y);
        mAccV.z = mAccelerometerFilter_Z.filterProcess(inputAcc.z);
        double a_f = mAccV.getNorm();
        mMagV.x = mMagneticFilter_X.filterProcess(inputMag.x);
        mMagV.y = mMagneticFilter_Y.filterProcess(inputMag.y);
        mMagV.z = mMagneticFilter_Z.filterProcess(inputMag.z);

   //     mGyrV = mGyr;

        //加速度计+磁力计航向估计
        getHeadingByAccMag();
        //陀螺仪航向估计
        getHeadingByGyr(inputGyr);

        //步态检测 步长估计
        Log.d(TAG, "inputData: 合加速度" + a_f);
        accStandardDeviation = mAccSTDCalculator.getStd(a_f);
        Log.d(TAG, "inputData: 标准差" + accStandardDeviation);
        if (accStandardDeviation > 2.5) {
            K = 0.5f;
            dynamicMinInterval = MIN_RUN_INTERVAL;
        } else {
            K = 0.4f;
            dynamicMinInterval = MIN_STEP_INTERVAL;
            if (dynamicInterval < dynamicMinInterval) dynamicInterval = MIN_STEP_INTERVAL;
        }
        if (a_f > maxSumAcc) {
            maxSumAcc = (float) a_f;
        } else if (a_f < minSumAcc) {
            minSumAcc = (float) a_f;
        }
        // Step Detect
        long currentTime = inputAcc.timestamp;
        if (a_f > STEP_TRIGGER_ACCEL) {
            long t = currentTime - mLastStepTime;
            if (t >= MAX_STEP_INTERVAL)
                dynamicInterval = INT_STEP_INTERVAL;
            else if (t > dynamicInterval) {
                //动态步长
                dynamicStepLength = (float) (K * Math.sqrt(Math.sqrt(maxSumAcc - minSumAcc)));
                //newStep();
                callback.onLocationChanged(dynamicStepLength, mOrientationResult);
                maxSumAcc = maxPeakThreshold;
                minSumAcc = minPeakThreshold;
            } else if (t <= dynamicInterval && t > dynamicMinInterval)
                dynamicInterval -= 50;
            else return;
            mLastStepTime = currentTime;
        }
    }

    private void getHeadingByGyr(Vector3Event inputGyr) {
        // This timestep's delta rotation to be multiplied by the current rotation
        // after computing it from the gyro sample data.
        if (!hasOrientation) {
            return;
        }
        // Initialization of the gyroscope based rotation matrix
        if (!initState) {
            gyroMatrix = matrixMultiplication(gyroMatrix, rotationMatrix);
            //       System.arraycopy(mOrientationValues, 0, fusedOrientation, 0, 3);
            initState = true;
        }

        if (timestamp != 0) {
            final float dT = (inputGyr.timestamp - timestamp) * NS2S;
            // Axis of the rotation sample, not normalized yet.
            float axisX = (float) inputGyr.x;
            float axisY = (float) inputGyr.y;
            float axisZ = (float) inputGyr.z;

            // Calculate the angular speed of the sample
            float omegaMagnitude = (float) Math.sqrt(Math.pow(axisX, 2) + Math.pow(axisY, 2) + Math.pow(axisZ, 2));

            // Normalize the rotation vector if it's big enough to get the axis
            // (that is, EPSILON should represent your maximum allowable margin of error)
            if (omegaMagnitude > EPSILON) {
                axisX /= omegaMagnitude;
                axisY /= omegaMagnitude;
                axisZ /= omegaMagnitude;
            }

            // Integrate around this axis with the angular speed by the timestep
            // in order to get a delta rotation from this sample over the timestep
            // We will convert this axis-angle representation of the delta rotation
            // into a quaternion before turning it into the rotation matrix.
            float thetaOverTwo = omegaMagnitude * dT / 2.0f;
            float sinThetaOverTwo = (float) Math.sin(thetaOverTwo);
            float cosThetaOverTwo = (float) Math.cos(thetaOverTwo);
            deltaRotationVector[0] = sinThetaOverTwo * axisX;
            deltaRotationVector[1] = sinThetaOverTwo * axisY;
            deltaRotationVector[2] = sinThetaOverTwo * axisZ;
            deltaRotationVector[3] = cosThetaOverTwo;
        }
        timestamp = mGyrV.timestamp;

        // Get the rotation matrix from the gyroscope
        SensorManager.getRotationMatrixFromVector(deltaRotationMatrix, deltaRotationVector);

        // Apply the new rotation interval on the gyroscope based rotation matrix to form a composite rotation matrix.
        // The product of two rotation matricies is a rotation matrix... Multiplication of rotation matrices
        // corresponds to composition of rotations... Which in this case are the rotation matrix from the fused
        // orientation and the rotation matrix from the current gyroscope outputs.
        gyroMatrix = matrixMultiplication(gyroMatrix, deltaRotationMatrix);

        // Get the gyroscope based orientation from the composite rotation matrix. This orientation will be fused via
        // complementary filter with the orientation from the acceleration sensor and magnetic sensor.
        SensorManager.getOrientation(gyroMatrix, gyroOrientation);


        // Log.d(TAG, "onSensorChanged: gyroOrientation:" + gyroOrientation[0] * 180 / Math.PI + " " + gyroOrientation[1] * 180 / Math.PI + " " + gyroOrientation[2] * 180 / Math.PI);

        calculateFusedOrientation();
        //    Log.d(TAG, "onSensorChanged: gyroOrientation1:" + fusedOrientation[0] * 180 / Math.PI + " ori:" + mOrientationValues[0] * 180 / Math.PI);
        newOrientation(fusedOrientation[0]);
    }

    private void getHeadingByAccMag() {
        if (mAccV.getNorm() != 0 && mMagV.getNorm() != 0) {
            if (SensorManager.getRotationMatrix(rotationMatrix, null, mAccV.toFloatArray(), mMagV.toFloatArray())) {
                //姿势1：手机+y轴竖直朝下，+z轴为前进方向
                SensorManager.remapCoordinateSystem(rotationMatrix, SensorManager.AXIS_X, SensorManager.AXIS_MINUS_Z, outR);
                SensorManager.getOrientation(outR, mOrientationValues);
                hasOrientation = true;

            }
        }
    }


    //  Multiply A by B
    private float[] matrixMultiplication(float[] A, float[] B) {
        float[] result = new float[9];

        result[0] = A[0] * B[0] + A[1] * B[3] + A[2] * B[6];
        result[1] = A[0] * B[1] + A[1] * B[4] + A[2] * B[7];
        result[2] = A[0] * B[2] + A[1] * B[5] + A[2] * B[8];

        result[3] = A[3] * B[0] + A[4] * B[3] + A[5] * B[6];
        result[4] = A[3] * B[1] + A[4] * B[4] + A[5] * B[7];
        result[5] = A[3] * B[2] + A[4] * B[5] + A[5] * B[8];

        result[6] = A[6] * B[0] + A[7] * B[3] + A[8] * B[6];
        result[7] = A[6] * B[1] + A[7] * B[4] + A[8] * B[7];
        result[8] = A[6] * B[2] + A[7] * B[5] + A[8] * B[8];

        return result;
    }

    /**
     * 将获得的新角度做滤波，之后更新UI图标
     *
     * @param radius 以弧度制表示的角度
     */
    private void newOrientation(double radius) {
        mSinFilter.filterProcess(Math.sin(radius));
        mCosFilter.filterProcess(Math.cos(radius));
        float deg = (float) Math.toDegrees(Math.asin(mSinFilter.get()));
        if (mSinFilter.get() > 0 && mCosFilter.get() < 0) deg = 180 - deg;
        if (mSinFilter.get() < 0 && mCosFilter.get() < 0) deg = -180 - deg;
        mOrientationResult = deg;
        callback.onOrientationChanged(mOrientationResult);
//
//        //TODO : 图标角度更新
//        Message msg = new Message();
//        msg.what = UPDATE_ORIENTATION_CODE;
//        Bundle b = new Bundle();
//        b.putDouble("newOrientation", mOrientationResult);
//        msg.setData(b);
//        MainActivity.sHandler.sendMessage(msg);
    }

//    private void newStep() {
//        //TODO:更新位置
//        Message msg = new Message();
//        msg.what = UPDATE_LOCATION_CODE;
//        Bundle b = new Bundle();
////        b.putDouble("stepLength", mStepLength);
//        b.putDouble("stepLength", dynamicStepLength);
//        b.putDouble("mOrientationResult", mOrientationResult);
//        msg.setData(b);
//        MainActivity.sHandler.sendMessage(msg);
//    }

    // Calculate the fused orientation.
    private void calculateFusedOrientation() {
        float oneMinusCoeff = (1.0f - FILTER_COEFFICIENT);
//        Log.d(TAG, "calculateFusedOrientation: oneMinusCoeff" + oneMinusCoeff);
//        Log.d(TAG, "calculateFusedOrientation: gyro:" + gyroOrientation[0] + " " + "mag_acc:" + mOrientationValues[0]);
        /*
         * Fix for 179° <--> -179° transition problem: Check whether one of the two orientation angles (gyro or
         * accMag) is negative while the other one is positive. If so, add 360° (2 * math.PI) to the negative value,
         * perform the sensor fusion, and remove the 360° from the result if it is greater than 180°. This stabilizes
         * the output in positive-to-negative-transition cases.
         */

        // azimuth
        if (gyroOrientation[0] < -0.5 * Math.PI && mOrientationValues[0] > 0.0) {
            fusedOrientation[0] = (float) (FILTER_COEFFICIENT * (gyroOrientation[0] + 2.0 * Math.PI) +
                    oneMinusCoeff * mOrientationValues[0]
            );
            fusedOrientation[0] -= (fusedOrientation[0] > Math.PI) ? 2.0 * Math.PI : 0;
        } else if (mOrientationValues[0] < -0.5 * Math.PI && gyroOrientation[0] > 0.0) {
            fusedOrientation[0] = (float) (FILTER_COEFFICIENT * gyroOrientation[0] + oneMinusCoeff *
                    (mOrientationValues[0] + 2.0 * Math.PI)
            );
            fusedOrientation[0] -= (fusedOrientation[0] > Math.PI) ? 2.0 * Math.PI : 0;
        } else {
            fusedOrientation[0] = FILTER_COEFFICIENT * gyroOrientation[0] + oneMinusCoeff * mOrientationValues[0];
        }

        // pitch
        if (gyroOrientation[1] < -0.5 * Math.PI && mOrientationValues[1] > 0.0) {
            fusedOrientation[1] = (float) (FILTER_COEFFICIENT * (gyroOrientation[1] + 2.0 * Math.PI) +
                    oneMinusCoeff * mOrientationValues[1]
            );
            fusedOrientation[1] -= (fusedOrientation[1] > Math.PI) ? 2.0 * Math.PI : 0;
        } else if (mOrientationValues[1] < -0.5 * Math.PI && gyroOrientation[1] > 0.0) {
            fusedOrientation[1] = (float) (FILTER_COEFFICIENT * gyroOrientation[1] + oneMinusCoeff *
                    (mOrientationValues[1] + 2.0 * Math.PI)
            );
            fusedOrientation[1] -= (fusedOrientation[1] > Math.PI) ? 2.0 * Math.PI : 0;
        } else {
            fusedOrientation[1] = FILTER_COEFFICIENT * gyroOrientation[1] + oneMinusCoeff * mOrientationValues[1];
        }

        // roll
        if (gyroOrientation[2] < -0.5 * Math.PI && mOrientationValues[2] > 0.0) {
            fusedOrientation[2] = (float) (FILTER_COEFFICIENT * (gyroOrientation[2] + 2.0 * Math.PI) +
                    oneMinusCoeff * mOrientationValues[2]
            );
            fusedOrientation[2] -= (fusedOrientation[2] > Math.PI) ? 2.0 * Math.PI : 0;
        } else if (mOrientationValues[2] < -0.5 * Math.PI && gyroOrientation[2] > 0.0) {
            fusedOrientation[2] = (float) (FILTER_COEFFICIENT * gyroOrientation[2] + oneMinusCoeff *
                    (mOrientationValues[2] + 2.0 * Math.PI)
            );
            fusedOrientation[2] -= (fusedOrientation[2] > Math.PI) ? 2.0 * Math.PI : 0;
        } else {
            fusedOrientation[2] = FILTER_COEFFICIENT * gyroOrientation[2] + oneMinusCoeff * mOrientationValues[2];
        }
        //   Log.d(TAG, "calculateFusedOrientation: gyro:fused:" + fusedOrientation[0]);
        // overwrite gyro matrix and orientation with fused orientation to comensate gyro drift
        gyroMatrix = getRotationMatrixFromOrientation(fusedOrientation);

        System.arraycopy(fusedOrientation, 0, gyroOrientation, 0, 3);
    }

    /*
     * Get the rotation matrix from the current orientation. Android Sensor
     * Manager does not provide a method to transform the orientation into a
     * rotation matrix, only the orientation from a rotation matrix. The basic
     * rotations can be found in Wikipedia with the caveat that the rotations
     * are *transposed* relative to what is required for this method.
     */
    private float[] getRotationMatrixFromOrientation(float[] orientation) {
        float[] xM = new float[9];
        float[] yM = new float[9];
        float[] zM = new float[9];

        float sinX = (float) Math.sin(orientation[1]);
        float cosX = (float) Math.cos(orientation[1]);
        float sinY = (float) Math.sin(orientation[2]);
        float cosY = (float) Math.cos(orientation[2]);
        float sinZ = (float) Math.sin(orientation[0]);
        float cosZ = (float) Math.cos(orientation[0]);

        // rotation about x-axis (pitch)
        xM[0] = 1.0f;
        xM[1] = 0.0f;
        xM[2] = 0.0f;
        xM[3] = 0.0f;
        xM[4] = cosX;
        xM[5] = sinX;
        xM[6] = 0.0f;
        xM[7] = -sinX;
        xM[8] = cosX;

        // rotation about y-axis (roll)
        yM[0] = cosY;
        yM[1] = 0.0f;
        yM[2] = sinY;
        yM[3] = 0.0f;
        yM[4] = 1.0f;
        yM[5] = 0.0f;
        yM[6] = -sinY;
        yM[7] = 0.0f;
        yM[8] = cosY;

        // rotation about z-axis (azimuth)
        zM[0] = cosZ;
        zM[1] = sinZ;
        zM[2] = 0.0f;
        zM[3] = -sinZ;
        zM[4] = cosZ;
        zM[5] = 0.0f;
        zM[6] = 0.0f;
        zM[7] = 0.0f;
        zM[8] = 1.0f;

        // Build the composite rotation... rotation order is y, x, z (roll, pitch, azimuth)
        float[] resultMatrix = matrixMultiplication(xM, yM);
        resultMatrix = matrixMultiplication(zM, resultMatrix);
        return resultMatrix;
    }
}
